#!/usr/bin/env python3
import sys
import threading
from PyQt5.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, 
                            QHBoxLayout, QLabel, QLineEdit, 
                            QPushButton, QWidget, QSlider)
from PyQt5.QtCore import Qt, QTimer
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

class RobotSteeringGUI(QMainWindow):
    def __init__(self, ros_node):
        super().__init__()
        self.ros_node = ros_node
        self.setWindowTitle("ROS 2 Robot Steering Controller")
        self.setGeometry(300, 300, 400, 250)
        
        # 存储当前控制指令
        self.current_cmd = Twist()
        self.publish_active = False

        # 主界面布局
        self.main_widget = QWidget()
        self.setCentralWidget(self.main_widget)
        self.layout = QVBoxLayout()
        self.main_widget.setLayout(self.layout)

        # 话题设置区域
        self.setup_topic_ui()
        
        # 速度控制区域
        self.setup_control_ui()
        
        # 初始化发布器
        self.cmd_vel_pub = self.ros_node.create_publisher(
            Twist, 
            self.topic_input.text(), 
            10
        )
        
        # 设置定时器持续发布 (10Hz)
        self.publish_timer = QTimer()
        self.publish_timer.timeout.connect(self.publish_cmd)
        self.publish_timer.start(100)  # 100ms间隔

    def setup_topic_ui(self):
        """设置话题名称输入区域"""
        topic_layout = QHBoxLayout()
        
        self.topic_label = QLabel("Control Topic:")
        self.topic_input = QLineEdit("/cmd_vel")
        self.update_topic_btn = QPushButton("Update")
        self.update_topic_btn.clicked.connect(self.update_topic)
        
        topic_layout.addWidget(self.topic_label)
        topic_layout.addWidget(self.topic_input)
        topic_layout.addWidget(self.update_topic_btn)
        self.layout.addLayout(topic_layout)

    def setup_control_ui(self):
        """设置速度控制区域"""
        # 线速度控制
        self.linear_label = QLabel("Linear Speed (m/s): 0.00")
        self.linear_slider = QSlider(Qt.Horizontal)
        self.linear_slider.setRange(-100, 100)
        self.linear_slider.valueChanged.connect(self.update_linear_speed)
        
        # 角速度控制
        self.angular_label = QLabel("Angular Speed (rad/s): 0.00")
        self.angular_slider = QSlider(Qt.Horizontal)
        self.angular_slider.setRange(-300, 300)
        self.angular_slider.valueChanged.connect(self.update_angular_speed)
        
        # 停止按钮
        self.stop_btn = QPushButton("EMERGENCY STOP")
        self.stop_btn.setStyleSheet("background-color: red; color: white;")
        self.stop_btn.clicked.connect(self.stop_robot)
        
        # 添加到布局
        self.layout.addWidget(self.linear_label)
        self.layout.addWidget(self.linear_slider)
        self.layout.addWidget(self.angular_label)
        self.layout.addWidget(self.angular_slider)
        self.layout.addWidget(self.stop_btn)

    def update_topic(self):
        """更新发布话题"""
        new_topic = self.topic_input.text().strip()
        if hasattr(self, 'cmd_vel_pub'):
            self.cmd_vel_pub.destroy()
        
        self.cmd_vel_pub = self.ros_node.create_publisher(
            Twist, 
            new_topic, 
            10
        )
        self.ros_node.get_logger().info(f"Publishing to: {new_topic}")

    def update_linear_speed(self, value):
        """更新线速度值"""
        speed = value / 100.0  # 转换为-1.0~1.0
        self.linear_label.setText(f"Linear Speed (m/s): {speed:.2f}")
        self.current_cmd.linear.x = speed
        self.publish_active = True

    def update_angular_speed(self, value):
        """更新角速度值"""
        speed = value / 100.0  # 转换为-3.0~3.0
        self.angular_label.setText(f"Angular Speed (rad/s): {speed:.2f}")
        self.current_cmd.angular.z = speed
        self.publish_active = True

    def publish_cmd(self):
        """定时发布控制指令"""
        if self.publish_active and self.cmd_vel_pub:
            self.cmd_vel_pub.publish(self.current_cmd)

    def stop_robot(self):
        """紧急停止"""
        self.linear_slider.setValue(0)
        self.angular_slider.setValue(0)
        self.current_cmd = Twist()  # 清零指令
        if self.cmd_vel_pub:
            self.cmd_vel_pub.publish(self.current_cmd)  # 立即发布停止指令
        self.publish_active = False

    def closeEvent(self, event):
        """窗口关闭时清理资源"""
        self.stop_robot()
        self.publish_timer.stop()
        event.accept()

def main(args=None):
    # 初始化ROS 2
    rclpy.init(args=args)
    ros_node = Node('robot_steering_gui')
    
    # 初始化Qt应用
    app = QApplication(sys.argv)
    
    try:
        # 创建GUI实例
        gui = RobotSteeringGUI(ros_node)
        gui.show()
        
        # 启动ROS事件循环线程
        spin_thread = threading.Thread(
            target=rclpy.spin, 
            args=(ros_node,), 
            daemon=True
        )
        spin_thread.start()
        
        # 运行Qt主循环
        app_exec = app.exec_()
        
    finally:
        # 清理资源
        ros_node.destroy_node()
        rclpy.shutdown()
    
    sys.exit(app_exec)

if __name__ == '__main__':
    main()